;/*
; * FreeRTOS Kernel V11.1.0
; * Copyright (C) 2021 Amazon.com, Inc. or its affiliates. All Rights Reserved.
; *
; * SPDX-License-Identifier: MIT
; *
; * Permission is hereby granted, free of charge, to any person obtaining a copy of
; * this software and associated documentation files (the "Software"), to deal in
; * the Software without restriction, including without limitation the rights to
; * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
; * the Software, and to permit persons to whom the Software is furnished to do so,
; * subject to the following conditions:
; *
; * The above copyright notice and this permission notice shall be included in all
; * copies or substantial portions of the Software.
; *
; * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
; * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
; * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
; * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
; * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
; * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
; *
; * https://www.FreeRTOS.org
; * https://github.com/FreeRTOS
; *
; */
        ; start: required for Cortex-R5 MPU port - generated by TI HALCoGen - see src/os/freertos/README.ti-halcogen.md for details */
        .sect ".kernelTEXT" ; stores OS at a specific address in memory
        .arm
        .ref vTaskSwitchContext
        .ref xTaskIncrementTick
        .ref ulTaskHasFPUContext
		.ref pxCurrentTCB
        .ref   ulCriticalNesting
        ; end: required for Cortex-R5 MPU port - generated by TI HALCoGen - see src/os/freertos/README.ti-halcogen.md for details */
;/*-----------------------------------------------------------*/
;
; Save Task Context
;
portSAVE_CONTEXT .macro
        DSB

        ; Push R0 as we are going to use it
        STMDB   SP!, {R0}

        ; start: required for Cortex-R5 MPU port - generated by TI HALCoGen - see src/os/freertos/README.ti-halcogen.md for details */
        ; Set R0 to point to the task stack pointer.
        SUB SP, SP, #4
        STMDA   SP,{SP}^
        ; end: required for Cortex-R5 MPU port - generated by TI HALCoGen - see src/os/freertos/README.ti-halcogen.md for details */
        LDMIA   SP!,{R0}

        ; Push the return address onto the stack.
        STMDB   R0!, {LR}

        ; Now LR has been saved, it can be used instead of R0.
        MOV LR, R0

        ; Pop R0 so it can be saved onto the task stack.
        LDMIA   SP!, {R0}

        ; Push all the system mode registers onto the task stack.
        STMDB   LR,{R0-LR}^
        SUB LR, LR, #60

        ; Push the SPSR onto the task stack.
        MRS R0, SPSR
        STMDB   LR!, {R0}

    .if (__TI_VFP_SUPPORT__)
        ;Determine if the task maintains an FPU context.
        LDR R0, ulFPUContextConst
        LDR R0, [R0]

        ; Test the flag
        CMP     R0, #0

        ; If the task is not using a floating point context then skip the
        ; saving of the FPU registers.
        BEQ     $+16
        FSTMDBD LR!, {D0-D15}
        FMRX    R1,  FPSCR
        STMFD   LR!, {R1}

        ; Save the flag
        STMDB   LR!, {R0}
    .endif

        ; Store the new top of stack for the task.
        LDR R0, pxCurrentTCBConst
        LDR R0, [R0]
        STR LR, [R0]

        .endm

;/*-----------------------------------------------------------*/
;
; Restore Task Context
;
portRESTORE_CONTEXT .macro
		LDR		R0, pxCurrentTCBConst
		LDR		R0, [R0]
        ; start: required for Cortex-R5 MPU port - generated by TI HALCoGen - see src/os/freertos/README.ti-halcogen.md for details
        ; task stack MPU region
        mov     r4,  #12                     ; Task Stack Region
        add     r12, r0, #4                 ; poin to regions in TCB
        ldmia   r12!, {r1-r3}
        mcr     p15, #0, r4, c6, c2, #0     ; Select region
        mcr     p15, #0, r1, c6, c1, #0     ; Base Address
        mcr     p15, #0, r3, c6, c1, #4     ; Access Attributes
        mcr     p15, #0, r2, c6, c1, #2     ; Size and Enable

        ldr     r5, portMax_MPU_Region
        mov     r4,  #13
        ; dynamic MPU per task
        ldmia   r12!, {r1-r3}
        mcr     p15, #0, r4, c6, c2, #0     ; Select region
        mcr     p15, #0, r1, c6, c1, #0     ; Base Address
        mcr     p15, #0, r3, c6, c1, #4     ; Access Attributes
        mcr     p15, #0, r2, c6, c1, #2     ; Size and Enable
        add     r4, r4, #1
        cmp     r4, r5
        bne     $ - 0x1C
        ; end: required for Cortex-R5 MPU port - generated by TI HALCoGen - see src/os/freertos/README.ti-halcogen.md for details
		LDR		LR, [R0]

    .if (__TI_VFP_SUPPORT__)
        ; The floating point context flag is the first thing on the stack.
        LDR     R0, ulFPUContextConst
        LDMFD   LR!, {R1}
        STR     R1, [R0]

        ; Test the flag
        CMP     R1, #0

        ; If the task is not using a floating point context then skip the
        ; VFP register loads.
        BEQ     $+16

        ; Restore the floating point context.
        LDMFD   LR!, {R0}
        FLDMIAD LR!, {D0-D15}
        FMXR    FPSCR, R0
    .endif

        ; Get the SPSR from the stack.
        LDMFD   LR!, {R0}
        MSR     SPSR_CSXF, R0

        ; Restore all system mode registers for the task.
        LDMFD   LR, {R0-R14}^

        ; Restore the return address.
        LDR     LR, [LR, #+60]
        ; start: required for Cortex-R5 MPU port - generated by TI HALCoGen - see src/os/freertos/README.ti-halcogen.md for details */
                DSB
        ; start: required for Cortex-R5 MPU port - generated by TI HALCoGen - see src/os/freertos/README.ti-halcogen.md for details */

        ; And return - correcting the offset in the LR to obtain the
        ; correct address.
        SUBS    PC, LR, #4
        .endm

; start: required for Cortex-R5 MPU port - generated by TI HALCoGen - see src/os/freertos/README.ti-halcogen.md for details
portMax_MPU_Region       .word   16 - 1
; end: required for Cortex-R5 MPU port - generated by TI HALCoGen - see src/os/freertos/README.ti-halcogen.md for details

;/*-----------------------------------------------------------*/
; Start the first task by restoring its context.

        .def vPortStartFirstTask
 ; start: required for Cortex-R5 MPU port - generated by TI HALCoGen - see src/os/freertos/README.ti-halcogen.md for details
        .asmfunc
vPortStartFirstTask
        cps #0x13
        portRESTORE_CONTEXT
        .endasmfunc
         ; end: required for Cortex-R5 MPU port - generated by TI HALCoGen - see src/os/freertos/README.ti-halcogen.md for details
;/*-----------------------------------------------------------*/
; Yield to another task.

        .def vPortYieldProcessor
 ; start: required for Cortex-R5 MPU port - generated by TI HALCoGen - see src/os/freertos/README.ti-halcogen.md for details
        .asmfunc ; helps us for debugging
swiPortYield
        ;  Restore stack and LR before calling vPortYieldProcessor
        ldmfd   sp!, {r11,r12,lr}
 ; end: required for Cortex-R5 MPU port - generated by TI HALCoGen - see src/os/freertos/README.ti-halcogen.md for details
vPortYieldProcessor:
        ; Within an IRQ ISR the link register has an offset from the true return
        ; address.  SWI doesn't do this. Add the offset manually so the ISR
        ; return code can be used.
        ADD     LR, LR, #4

        ; First save the context of the current task.
        portSAVE_CONTEXT

        ; Select the next task to execute. */
        BL      vTaskSwitchContext

        ; Restore the context of the task selected to execute.
        portRESTORE_CONTEXT
; start: required for Cortex-R5 MPU port - generated by TI HALCoGen - see src/os/freertos/README.ti-halcogen.md for details
        .endasmfunc ; helps us for debugging
 ; end: required for Cortex-R5 MPU port - generated by TI HALCoGen - see src/os/freertos/README.ti-halcogen.md for details

;/*-----------------------------------------------------------*/
; Yield to another task from within the FreeRTOS API

	.def vPortYieldWithinAPI
; start: required for Cortex-R5 MPU port - generated by TI HALCoGen - see src/os/freertos/README.ti-halcogen.md for details
        .asmfunc ; helps us for debugging
; end: required for Cortex-R5 MPU port - generated by TI HALCoGen - see src/os/freertos/README.ti-halcogen.md for details
vPortYieldWithinAPI
		; Save the context of the current task.

        portSAVE_CONTEXT
        ; Clear SSI flag.
        MOVW    R0, #0xFFF4
        MOVT    R0, #0xFFFF
        LDR     R0, [R0]

        ; Select the next task to execute. */
        BL      vTaskSwitchContext

        ; Restore the context of the task selected to execute.
        portRESTORE_CONTEXT
 ; start: required for Cortex-R5 MPU port - generated by TI HALCoGen - see src/os/freertos/README.ti-halcogen.md for details
        .endasmfunc ; helps us for debugging
 ; end: required for Cortex-R5 MPU port - generated by TI HALCoGen - see src/os/freertos/README.ti-halcogen.md for details
;/*-----------------------------------------------------------*/
; Preemptive Tick

        .def vPortPreemptiveTick
 ; start: required for Cortex-R5 MPU port - generated by TI HALCoGen - see src/os/freertos/README.ti-halcogen.md for details
        .asmfunc ; helps us for debugging
 ; end: required for Cortex-R5 MPU port - generated by TI HALCoGen - see src/os/freertos/README.ti-halcogen.md for details
vPortPreemptiveTick:

        ; Save the context of the current task.
        portSAVE_CONTEXT

        ; Clear interrupt flag
        MOVW    R0, #0xFC88
        MOVT    R0, #0xFFFF
        MOV     R1, #1
        STR     R1, [R0]

        ; Increment the tick count, making any adjustments to the blocked lists
        ; that may be necessary.
        BL      xTaskIncrementTick

        ; Select the next task to execute.
        CMP R0, #0
        BLNE    vTaskSwitchContext

        ; Restore the context of the task selected to execute.
        portRESTORE_CONTEXT
; start: required for Cortex-R5 MPU port - generated by TI HALCoGen - see src/os/freertos/README.ti-halcogen.md for details
        .endasmfunc ; helps us for debugging
; end: required for Cortex-R5 MPU port - generated by TI HALCoGen - see src/os/freertos/README.ti-halcogen.md for details

;-------------------------------------------------------------------------------

    .if (__TI_VFP_SUPPORT__)

        .def vPortInitialiseFPSCR

vPortInitialiseFPSCR:

        MOV     R0, #0
        FMXR    FPSCR, R0
        BX      LR

	.endif ;__TI_VFP_SUPPORT__
; start: required for Cortex-R5 MPU port - generated by TI HALCoGen - see src/os/freertos/README.ti-halcogen.md for details
;-------------------------------------------------------------------------------
; SWI Handler, interface to Protected Mode Functions

; Every time the swi handler is called an exeption is generated. Then the CPSR is stored
; in the SPSR and restored afterwards from there. That is why this register has to be manipulated.
; See:  https://developer.arm.com/documentation/ddi0406/c/System-Level-Architecture/The-System-Level-Programmers--Model/ARM-processor-modes-and-ARM-core-registers/Program-Status-Registers--PSRs-?lang=en#CIHHHBDD
;       https://developer.arm.com/documentation/102412/0103/Exception-types
;       https://developer.arm.com/documentation/dui0801/f/A32-and-T32-Instructions/BLX--BLXNS

        .def    vPortSWI
        .asmfunc
vPortSWI
        stmfd   sp!, {r11,r12,lr}
        ldrb    r12, [lr, #-1]
        ldr     r14,  table
        ldr     r12, [r14, r12, lsl #2]
        blx     r12
        ldmfd   sp!, {r11,r12,pc}^

table
        .word    jumpTable

jumpTable
        .word   swiPortYield             ; 0 - vPortYieldProcessor
        .word   swiRaisePrivilege        ; 1 - xPortRaisePrivilege
        .word   swiPortEnterCritical     ; 2 - vPortEnterCritical
        .word   swiPortExitCritical      ; 3 - vPortExitCritical
        .word   swiPortTaskUsesFPU       ; 4 - vPortTaskUsesFPU
        .word   swiPortDisableInterrupts ; 5 - vPortDisableInterrupts
        .word   swiPortEnableInterrupts  ; 6 - vPortEnableInterrupts
        .endasmfunc
;-------------------------------------------------------------------------------
; swiPortDisableInterrupts
        .asmfunc
swiPortDisableInterrupts
        mrs     r11, SPSR
        orr     r11, r11, #0x80
        msr     SPSR_c, r11
        bx      r14
        .endasmfunc
;-------------------------------------------------------------------------------
; swiPortEnableInterrupts
        .asmfunc
swiPortEnableInterrupts
        mrs     r11, SPSR
        bic     r11, r11, #0x80
        msr     SPSR_c, r11
        bx      r14
        .endasmfunc
;-------------------------------------------------------------------------------
; swiPortTaskUsesFPU
        .asmfunc
swiPortTaskUsesFPU
        ldr     r12, ulTaskHasFPUContextConst
        mov     r11, #1
        str     r11, [r12]
        mov     r11, #0
        fmxr    FPSCR, r11
        bx      r14

        .endasmfunc


;-------------------------------------------------------------------------------
; swiRaisePrivilege

; Must return zero in R0 if caller was in user mode
        .asmfunc
swiRaisePrivilege
        mrs     r12, spsr
        ands    r0, r12, #0x0F      ; return value
        orreq   r12, r12, #0x1F
        msreq   spsr_c, r12
        bx      r14
        .endasmfunc
;-------------------------------------------------------------------------------
; swiPortEnterCritical
        .asmfunc
swiPortEnterCritical
        mrs     r11, SPSR
        orr     r11, r11, #0x80
        msr     SPSR_c, r11
        ldr     r11, ulCriticalNestingConst
        ldr     r12, [r11]
        add     r12, r12, #1
        str     r12, [r11]
        bx      r14
        .endasmfunc
;-------------------------------------------------------------------------------
; swiPortExitCritical
        .asmfunc
swiPortExitCritical
        ldr     r11, ulCriticalNestingConst
        ldr     r12, [r11]
        cmp     r12, #0
        bxeq    r14
        subs    r12, r12, #1
        str     r12, [r11]
        bxne    r14
        mrs     r11, SPSR
        bic     r11, r11, #0x80
        msr     SPSR_c, r11
        bx      r14
        .endasmfunc
;-------------------------------------------------------------------------------
; SetRegion

        .def  prvMpuSetRegion
        .asmfunc
; void _mpuSetRegion(unsigned region, unsigned base, unsigned size, unsigned access);

prvMpuSetRegion
        and   r0,  r0, #15                  ; select region
        mcr   p15, #0, r0, c6, c2, #0
        mcr   p15, #0, r1, c6, c1, #0       ; Base Address
        mcr   p15, #0, r3, c6, c1, #4       ; Access Attributes
        mcr   p15, #0, r2, c6, c1, #2       ; Size and Enable
        bx    lr
        .endasmfunc
;-------------------------------------------------------------------------------
; Enable Mpu

        .def  prvMpuEnable
        .asmfunc
prvMpuEnable
        mrc   p15, #0, r0, c1, c0, #0
        orr   r0,  r0, #1
        dsb
        mcr   p15, #0, r0, c1, c0, #0
        isb
        bx    lr
        .endasmfunc
;-------------------------------------------------------------------------------
; Disable Mpu

        .def  prvMpuDisable
        .asmfunc
prvMpuDisable
        mrc   p15, #0, r0, c1, c0, #0
        bic   r0,  r0, #1
        dsb
        mcr   p15, #0, r0, c1, c0, #0
        isb
        bx    lr
        .endasmfunc
;-------------------------------------------------------------------------------
; end: required for Cortex-R5 MPU port - generated by TI HALCoGen - see src/os/freertos/README.ti-halcogen.md for details
pxCurrentTCBConst	.word	pxCurrentTCB
ulFPUContextConst 	.word   ulTaskHasFPUContext
; start: required for Cortex-R5 MPU port - generated by TI HALCoGen - see src/os/freertos/README.ti-halcogen.md for details
ulCriticalNestingConst   .word   ulCriticalNesting
ulTaskHasFPUContextConst .word   ulTaskHasFPUContext
; end: required for Cortex-R5 MPU port - generated by TI HALCoGen - see src/os/freertos/README.ti-halcogen.md for details
;-------------------------------------------------------------------------------
